#include<iostream>
#include<chrono>
#include<vector>
#include<queue>
#include<thread>
#include<mutex>

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "nav_msgs/Odometry.h"
#include "sensor_msgs/Imu.h"

#include "opencv2/core/core.hpp"

#include "../include/System.h"
#include "../include/ImuTypes.h"

using namespace std;

class ImuGrabber
{
public:
    ImuGrabber(){};
    void GrabImu(const sensor_msgs::ImuConstPtr &imu_msg);

    queue<sensor_msgs::ImuConstPtr> imuBuf;
    std::mutex mBufMutexImu;
};

class OdomGrabber
{
public:
    OdomGrabber(msf::System *pMSFL, ImuGrabber *pImuGb): mpMSFL(pMSFL), mpImuGb(pImuGb){};
    void GrabOdom(const nav_msgs::OdometryConstPtr &odom_msg);
    void SyncWithImu();

    queue<nav_msgs::OdometryConstPtr> odomBuf;
    std::mutex mBufMutexOdom;

    msf::System *mpMSFL;
    ImuGrabber *mpImuGb;
};

//2024/04/17
//trick:在imu初始化阶段完成后新增的大量关键位姿帧，接着进行一次全局优化，此时叠加机器人本身运动中的前端eskf定位推算，此处关键帧的优化更新处理易导致当前位姿估计产生偏差。
int main(int argc, char **argv)
{
    ros::init(argc,argv,"imu_odom_fusion");
    ros::NodeHandle n("~");
    
    if(argc < 2 || argc > 2)
    {
        cerr << endl << "argc error: Usage: rosrun imu_odom_fusion imu_odom path_to_settings!!" << endl;
        ros::shutdown();
        return 1;
    }

    // 创建定位系统。初始化所有线程并准备好处理数据帧
    msf::System msfl(argv[1], msf::System::IMU_ODOM, true);

    ImuGrabber imugb;
    OdomGrabber odomgb(&msfl, &imugb);

    // Maximum delay, 5 seconds
    ros::Subscriber sub_imu = n.subscribe("/imu/data", 1000, &ImuGrabber::GrabImu, &imugb); 
    ros::Subscriber sub_odom = n.subscribe("/odom", 100, &OdomGrabber::GrabOdom, &odomgb);//<nav_msgs::Odometry>

    std::thread sync_thread(&OdomGrabber::SyncWithImu, &odomgb);
    ros::spin();//自循环等待消息进入回调函数
    return 0;
}

void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg)
{
    mBufMutexImu.lock();
    imuBuf.push(imu_msg);
    mBufMutexImu.unlock();
    return;
}

void OdomGrabber::GrabOdom(const nav_msgs::OdometryConstPtr &odom_msg)
{
    mBufMutexOdom.lock();
    //if (!odomBuf.empty())
        //odomBuf.pop();
    odomBuf.push(odom_msg);
    mBufMutexOdom.unlock();
    return;
}

void OdomGrabber::SyncWithImu()
{
    while(1)
    {
        double tOdom = 0;
        nav_msgs::OdometryConstPtr Odom; // 存储当前odom数据
        if(!odomBuf.empty()&&!mpImuGb->imuBuf.empty())
        {
            tOdom = odomBuf.front()->header.stamp.toSec();

            if(tOdom>mpImuGb->imuBuf.back()->header.stamp.toSec())//Odom帧的时间要在imu数据包时间中
                continue;                                         //当前图像数据还没有对应的IMU数据，需要等待新的IMU数据到来才能进行处理
            
            this->mBufMutexOdom.lock();
            Odom = odomBuf.front(); // front为时间上最早的元素
            odomBuf.pop();
            this->mBufMutexOdom.unlock();

            vector<msf::IMU::Point> vImuMeas; // 存储当前IMU数据
            mpImuGb->mBufMutexImu.lock();
            if(!mpImuGb->imuBuf.empty())
            {
                // 从buffer中加载imu测量值
                vImuMeas.clear();
                while(!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.toSec()<=tOdom)
                {
                    double t = mpImuGb->imuBuf.front()->header.stamp.toSec();
                    cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z);
                    cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z);
                    vImuMeas.push_back(msf::IMU::Point(acc,gyr,t));
                    mpImuGb->imuBuf.pop();
                }
            }
            mpImuGb->mBufMutexImu.unlock();

            mpMSFL->Track(Odom, tOdom, vImuMeas);

            std::chrono::milliseconds tSleep(1);
            std::this_thread::sleep_for(tSleep);
        }
    }
    
}

